#include "LogicJudge.h"

uint16_t LogicSpdCtrlMode = 1;//电机调速模式 0:旋钮 1:串口报文
uint16_t LogicPotValue = 0;//旋钮换算得出的控制值
uint16_t LogicPotAimSpd = 0;//旋钮换算得出的目标速度
uint16_t LogicMessageCCR = 0;
uint16_t LogicMessageSpd = 0;

typedef struct 
{
    uint16_t SpdCtrlMode;//电机调速模式 0:旋钮 1:串口报文
    uint16_t PotValue;//旋钮换算得出的控制值
    uint16_t PotAimSpd;//旋钮换算得出的目标速度
    uint16_t MessageCCR;
    uint16_t MessageSpd;    
}LogicConfig_t;

PiInfoQ15_t xMotorSPEEDD_PID;

LogicConfig_t xLogicConfigInfo;

LadderValue_t xLadderValueInfo = {.AimValue = 0,.CurrentValue = 0,.Delta = DUTY_MIN_DELTA,
                                  .MaxDelta = 10 ,.MinValue = 0};//斜坡值调速结构体


static void MotorRunStopProcess(void);
static void updatePotAdcValueProcess(void);
static void updateMotorCCR(void);

void LogicJudgeInit(void)
{
    xMotorSPEEDD_PID.Kp_Q15 = 16384;
    xMotorSPEEDD_PID.Ki_Q15 = 64;
    xMotorSPEEDD_PID.Kc_Q15 = 32440;
    xMotorSPEEDD_PID.OutMax_Q15 = 32767;
    xMotorSPEEDD_PID.OutMin_Q15 = 620;    
    xMotorSPEEDD_PID.ErrSum_Q30 = 0;
}

void vLogicJudgeProcess(void)
{
    updatePotAdcValueProcess();//旋钮值换算成速度控制值
    MotorRunStopProcess();//电机启停状态切换
    updateMotorCCR();//更新电机CCR值
}

uint16_t u16LogicGetPotValue(void)
{
    return LogicPotValue;
}

//电机启停控制流程
static void MotorRunStopProcess(void)
{
    MotorState_t eMotorState = eMotorGetState();//获取电机状态
    uint32_t PotValue = u16LogicGetPotValue();//获取旋钮控制值

    switch( (SpeedCtrlMode_t)LogicSpdCtrlMode )
    {
        case POT_CTRL://旋钮控制模式
        {
            if( (MotorCtrlMode_t)u16GetControlMode() == OPEN_LOOP )//开环
            {
                if( eMotorState == MotorStStop && PotValue > 50 && protectFaultBitMap==0 )
                {
                    MotorSetState(MotorStChargeReady);//停止状态下，旋钮拧到一定位置，开始给自举电容充电
                }
                else if( (eMotorState != MotorStStop && PotValue < 40) || protectFaultBitMap!=0 )//宽度为10的滞回窗口
                {
                    MotorSetState(MotorStStop);//运行状态下，旋钮回到一定位置，控制电机停止
                    //MotorSetCCR(0);
                    //LogicPotValue = 0;
                }
            }
            else if( (MotorCtrlMode_t)u16GetControlMode() == CLOSE_LOOP )//闭环
            {
                if( eMotorState == MotorStStop && LogicPotAimSpd > 80 && protectFaultBitMap==0 )
                {
                    MotorSetState(MotorStChargeReady);//停止状态下，旋钮拧到一定位置，开始给自举电容充电
                }
                else if( (eMotorState != MotorStStop && LogicPotAimSpd < 60) )//宽度为10的滞回窗口
                {
                    //MotorSetState(MotorStStop);//运行状态下，旋钮回到一定位置，控制电机停止
                    //MotorSetCCR(0);
                    //LogicPotValue = 0;
                    LogicPotAimSpd = 60;
                }

                if( protectFaultBitMap!=0 )
                    MotorSetState(MotorStStop);
            }            
            break;           
        }
        case MESSAGE_CTRL://报文控制模式
        {
            if( (MotorCtrlMode_t)u16GetControlMode() == OPEN_LOOP )//开环
            {
                if( eMotorState == MotorStStop && LogicMessageCCR > 50 && protectFaultBitMap==0 )
                {
                    MotorSetState(MotorStChargeReady);//停止状态下，给一定的占空比，开始给自举电容充电
                }
                else if( (eMotorState != MotorStStop && LogicMessageCCR < 40) || protectFaultBitMap!=0 )//宽度为10的滞回窗口
                {
                    MotorSetState(MotorStStop);//运行状态下，小于一定的占空比，控制电机停止
                    //MotorSetCCR(0);                    
                }                
            }
            else if( (MotorCtrlMode_t)u16GetControlMode() == CLOSE_LOOP )//闭环
            {
                if( eMotorState == MotorStStop && LogicMessageSpd > 60 && protectFaultBitMap==0 )
                {
                    MotorSetState(MotorStChargeReady);//停止状态下，给一定的转速，开始给自举电容充电
                }
                else if( (eMotorState != MotorStStop && LogicMessageSpd <= 50) || protectFaultBitMap!=0 )
                {
                    MotorSetState(MotorStStop);//运行状态下，小于一定的转速，控制电机停止
                }
            }
            break;
        }
        default:
            break;
    }
}

static void updateMotorCCR(void)
{
    uint16_t u16NewCCR;
    uint16_t newAimValue;
    uint16_t newAimSpd;
    if( MotorStRun != eMotorGetState() ){
        return;
    }

    if( 0 == u32GetDutySetCnt() )
    {
        if( (MotorCtrlMode_t)u16GetControlMode() == OPEN_LOOP )//开环
        {
            if( LogicSpdCtrlMode ==  POT_CTRL ){
                newAimValue = LogicPotValue;
            }else if( LogicSpdCtrlMode == MESSAGE_CTRL ){
                newAimValue = LogicMessageCCR;
            }

            if( newAimValue > (RUN_STATE_MAX_DUTY * 20) ){
                newAimValue = RUN_STATE_MAX_DUTY * 20;//目标值不得高于预设的最大值
            }

            xLadderValueInfo.AimValue = newAimValue;
            u16NewCCR = LadderValueSet(&xLadderValueInfo);//计算得出新的CCR斜坡值
            xLadderValueInfo.CurrentValue = u16NewCCR;

            if( u16NewCCR == newAimValue ){
                xLadderValueInfo.Delta = DUTY_MIN_DELTA;
            }

        }
        else if( (MotorCtrlMode_t)u16GetControlMode() == CLOSE_LOOP )
        {
            if( LogicSpdCtrlMode ==  POT_CTRL ){
                newAimSpd = LogicPotAimSpd;
            }else if( LogicSpdCtrlMode == MESSAGE_CTRL ){
                newAimSpd = LogicMessageSpd;
            }
            
            xLadderValueInfo.AimValue = newAimSpd;
            //newAimSpd = LadderValueSet(&xLadderValueInfo);//计算得出新的速度斜坡值0 ~ 3276
            xLadderValueInfo.CurrentValue = newAimSpd;

            xMotorSPEEDD_PID.Ref_Q15 = newAimSpd * 10;//通过乘以10归一化，0 ~ 32760

            if( xMotorMode.speedAct > 3276 ){
                xMotorMode.speedAct = 3276;
            }

            xMotorSPEEDD_PID.Act_Q15 = (xMotorMode.speedAct) * 10;//通过乘以10归一化，0 ~ 32760
         
            PiControl_AllQ15(&xMotorSPEEDD_PID);

            u16NewCCR = (xMotorSPEEDD_PID.Out_Q15 * 2000) >> 15;//Q15格式的0~1，放大至0~2000
        }

        if( !xMotorMode.speedContEnable ){
            u16NewCCR = 400;
        }

        MotorSetCCR(u16NewCCR);
        setDutySetCnt(DUTY_CHANGE_PERIOD_MS);                
    }  
}

//将旋钮ADC值换算成控制值
static void updatePotAdcValueProcess(void)
{
    uint16_t Q16Temp;
    uint16_t u16PotAdcVal = u16AdcGetAverageValue(AN_DEF_POT);//获取旋钮ADC平均值,0~4095
    /*换算开环模式下使用的CCR值*/
    LogicPotValue = u16PotAdcVal>>1;//移位代替除法，0~2047
    if( LogicPotValue>2000 ){
        LogicPotValue = 2000;//限幅，0~2000
    }
    /*换算闭环模式下使用的速度值 0 ~ 3276*/
    //AimSpeed/3276 = u16PotAdcVal/4095 => AimSpeed = u16PotAdcVal * 3276 / 4095 =>
    //AimSpeed = u16PotAdcVal * 16 *3276 / 4095 *16 = u16PotAdcVal * 16 *3276 / 65520 ≈ u16PotAdcVal * 16 *3276 / 65535

    Q16Temp = u16PotAdcVal << 4 ;
    LogicPotAimSpd = Q16Temp * 3276 >> 16;

    if( LogicPotAimSpd > 3276 ){
        LogicPotAimSpd = 3276;
    }

}
